﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using Microsoft.Robotics.Services.DepthCamSensor;

namespace NestedParticleFilter
{
    /// <summary>
    /// Motion class including rotational and transitional velocity
    /// </summary>
    public class Motions
    {
        private double _velocity; // meters/sec
        private double _omega;  // degree/sec

        /// <summary>
        /// velcoity (meters/sec)
        /// </summary>
        public double Velocity
        {
            set { _velocity = value; }
            get { return _velocity; }
        }

        /// <summary>
        /// rotational velocity (degree/sec)
        /// </summary>
        public double Omega
        {
            set { _omega = value; }
            get { return _omega; }
        }

        public Motions()
        {
        }

        /// <summary>
        /// Sets the velocity and rotational velocity to the given values. The scales are 
        /// meters/sec and degrees/sec respectively
        /// </summary>
        /// <param name="v">velocity m/s</param>
        /// <param name="w">rotational velocity degree/sec</param>
        public Motions(double v, double w)
        {
            _velocity = v;
            _omega = w;

        }

        public override string ToString()
        {
            return "( " + this._velocity + ", " + this._omega + ")";
        }

    }

    /// <summary>
    /// Parameter class for queuing the actions and observations
    /// </summary>
    public class Parameter
    {
        private Motions _action;   // action
        private double _time;      // time elapsed to take the action in seconds
        private DepthCamSensorState _depthState;
        private RobotPose _realRobotPose; // for testing purposes
        private List<IdentifiedLandmark> _identifiedLandmarks;
        private bool _identifiedRobot;
        private double _robotDepth;
        private double _robotAngle;
        private CustomisedColor _robotColor;
        private DateTime _timeQueued;
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="motion">Motion model</param>
        /// <param name="time">time in seconds</param>
        /// <param name="depthState">depth cam state</param>
        /// <param name="r"></param>
        public Parameter(Motions motion, double time, DepthCamSensorState depthState, RobotPose r)
        {
            _action = motion;
            _time = time;
            _depthState = depthState;
            //_image = bmp;
            _realRobotPose = r;

        }
        /// <summary>
        /// Action taken by the robot (Rotational and transitional velocity)
        /// </summary>
        public Motions Action
        {
            set { _action = value; }
            get { return _action; }
        }

        /// <summary>
        /// The time that took to perform an action in seconds
        /// </summary>
        public double Time
        {
            set { _time = value; }
            get { return _time; }
        }
        /// <summary>
        /// Depth cam state
        /// </summary>
        public DepthCamSensorState DepthState
        {
            set { _depthState = value; }
            get { return _depthState; }
        }
        ///// <summary>
        ///// bitmap taken from the scene for observation purposes
        ///// </summary>
        //public Bitmap Image
        //{
        //    set { _image = value; }
        //    get { return _image; }
        //}

        public RobotPose RealRobotPose
        {
            set { _realRobotPose = value; }
            get { return _realRobotPose; }
        }

        public List<IdentifiedLandmark> IdentifiedLandmarks
        {
            set { _identifiedLandmarks = value; }
            get { return _identifiedLandmarks; }
        }

        public bool IdentifiedRobot
        {
            set { _identifiedRobot = value; }
            get { return _identifiedRobot; }
        }

        public double RobotDepth
        {
            set { _robotDepth = value; }
            get { return _robotDepth; }
        }
        public double RobotAngle
        {
            set { _robotAngle = value; }
            get { return _robotAngle; }
        }
        public CustomisedColor RobotColor
        {
            set { _robotColor = value;}
            get { return _robotColor; }
        }
        public DateTime TimeQueued
        {
            set { _timeQueued = value; }
            get { return _timeQueued; }
        }
        public override string ToString()
        {
            return "Motion: " + _action + " time: " + _time;
        }
    }


    public class UnitConversion
    {
        private const float HundredEighty = 180;
        /// <summary>
        /// Converts an angle in Degree to Radian
        /// </summary>
        /// <param name="angle">Angle in degree </param>
        /// <returns></returns>
        public static double DegreeToRadian(double angle)
        {

            return angle * Math.PI / 180;
        }

        /// <summary>
        /// Converts an angle from Radoam to degree
        /// </summary>
        /// <param name="angle">angle in radian</param>
        /// <returns></returns>
        public static double RadianToDegree(double angle)
        {
            double degree = (angle * HundredEighty) / Math.PI;
            if (degree >= HundredEighty)
                degree = degree - 2 * HundredEighty;
            else if (degree < -HundredEighty)
                degree = degree + 2 * HundredEighty;
            return degree;
        }

        /// <summary>
        /// It puts the theta (in radian)  in bound . Theta in this simulator ranges between [-pi, pi]
        /// </summary>
        /// <param name="theta">theta in radian</param>
        /// <returns>theta in bound</returns>
        public static double PutThetaInBound(double theta)
        {
            // if theta is out of bound ( greater than 180 or less than -180) put it in the bound
            // theta should be between -180 and 180

            if (theta <= -Math.PI) //ex: -270
                while (theta <= -Math.PI)
                    theta = 2 * Math.PI + theta;
            if (theta >= Math.PI)  // ex:270
                while (theta >= Math.PI)
                    theta = theta - 2 * Math.PI;
            return theta;

        }
    }
}
